
#include <level/slide/object/kinematic/kinematic.hpp>
#include <window/window.hpp>
#include <nmn/create_string.hpp>

kinematic_object::kinematic_object( rapidxml::xml_node<>* node, world_ptr world )
	:	m_max_time( 1.0f )
{
	load_animation( node );

	b2BodyDef definition;

	b2Vec2 initial_position = definition.position;
	float32 initial_angle = definition.angle;

	if ( rapidxml::xml_node<> * points = node->first_node( "points" ) )
	{
		for (	rapidxml::xml_node<> * point_node = points->first_node( "point" );
				point_node != nullptr;
				point_node = point_node->next_sibling( point_node->name( ) ) )
		{
			point_t point(	g_inv_scale_b2_position * read_from_node<b2Vec2>( point_node->first_node( "position" ), initial_position ),
							b2DegToRad( read_from_node<float32>( point_node->first_node( "angle" ), initial_angle ) ) );

			float time = read_from_node<float>( point_node->first_node( "time" ), -1.0f );

			if ( time >= 0.0f )
			{
				auto iter = m_targets.lower_bound( time );
				if ( iter == m_targets.end( ) || iter->first != time )
				{
					m_targets.insert( iter, std::make_pair( time, point ) );
					m_max_time = b2Max( m_max_time, time );
				}
			}
		}

		if ( !m_targets.empty( ) )
		{
			initial_position = m_targets.begin( )->second.first;
			initial_angle = m_targets.begin( )->second.second;
			m_current_time = m_targets.begin( )->first;
		}
	}

	definition.type = b2_kinematicBody;
	definition.position = initial_position;
	definition.angle = read_from_node<float32>( node->first_node( "angle" ), definition.angle );
	definition.allowSleep = true;

	m_body = create_body( world, &definition );
	assign_user_data( );

	if ( rapidxml::xml_node<> * shapes = node->first_node( "shapes" ) )
	{
		for (	rapidxml::xml_node<>* shape = shapes->first_node( "shape" );
				shape != nullptr;
				shape = shape->next_sibling( shape->name( ) ) )
		{
			add_shape( shape );
		}
	}
}

void kinematic_object::step( float delta_time )
{
	object::step( delta_time );

	if ( !m_targets.empty( ) )
	{
		m_current_time += delta_time;
		m_current_time = fmodf( m_current_time, m_max_time );

		auto upper_bound = m_targets.upper_bound( m_current_time );
		float time_to_reach_target = upper_bound->first - m_current_time;
		if ( upper_bound == m_targets.end( ) )
		{
			upper_bound = m_targets.begin( );
			time_to_reach_target = upper_bound->first + ( m_max_time - m_current_time );
		}

		b2Vec2 const & current_position = m_body->GetPosition( );
		b2Vec2 const & target_position = upper_bound->second.first;
		b2Vec2 const required_movement = target_position - current_position;

		float32 const current_rotation = m_body->GetAngle( );
		float32 const & target_rotation = upper_bound->second.second;
		float32 const required_rotation = target_rotation - current_rotation;

		b2Vec2 const required_velocity = ( 1.0f / time_to_reach_target ) * required_movement;
		float32 required_angular_velocity = ( 1.0f / time_to_reach_target ) * required_rotation;

		m_body->SetLinearVelocity( required_velocity );
		m_body->SetAngularVelocity( required_angular_velocity );
	}
}

//void kinematic_object::draw( )
//{
//	object::draw( );
//
//	auto upper_bound = m_targets.upper_bound( m_current_time );
//	float time_to_reach_target = upper_bound->first - m_current_time;
//	if ( upper_bound == m_targets.end( ) )
//	{
//		upper_bound = m_targets.begin( );
//		time_to_reach_target = upper_bound->first + ( m_max_time - m_current_time );
//	}
//
//	sf::String output( nmn::create_string( "%1% / %2%\n%3%", m_current_time, m_max_time, time_to_reach_target ) );
//	output.SetColor( sf::Color::Red );
//	output.SetPosition( g_scale_b2_position * upper_bound->second.first.x, g_scale_b2_position * upper_bound->second.first.y );
//
//	g_window->Draw( output );
//}
